#ifndef IM_JOINT_STATE_PUBLISHER_H
#define IM_JOINT_STATE_PUBLISHER_H

#include <im_joint_state_publisher/im_joint.h>
#include <fstream>
#include <tf/transform_broadcaster.h>
#include <resource_retriever/retriever.h>
#include <assimp/assimp.hpp>     
#include <assimp/aiScene.h>      
#include <assimp/aiPostProcess.h>

namespace im_joint_state_publisher {

class ImJointStatePublisher {
public:
	ImJointStatePublisher();

	~ImJointStatePublisher();

	bool loadNewRobot();
	bool loadNewRobot(const std::string filename);
//	bool loadPreshape(const std::string filename);
//	bool savePreshape(const std::string filename);

	std::vector<boost::shared_ptr<urdf::Link> > links_list;
	std::vector<im_joint_state_publisher::ImJoint> joint_list;
	std::string root;
	std::string tf_prefix;

private:
	interactive_markers::InteractiveMarkerServer* server;
	int count;
	std::map<std::string, int> joint;
	std::string robot_name;
	urdf::Model robot_model;
	ros::NodeHandle node;
	ros::Timer timer;
	ros::Publisher joint_states;

	void updateJointsAndMarkers(const ros::TimerEvent& e);
	void clear();
	bool createJointMarker();
	bool checkLink(const std::string);
	bool checkJoint(const std::string);
	void processFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback);
	double constructShape(const urdf::Geometry *geom);
	double createMeshFromFilename(const std::string& filename,const tf::Vector3* scale);
	double createMeshFromAsset(const aiMesh* a, const aiMatrix4x4& transform,const tf::Vector3& scale);
};

}
//// namespace im_joint_state_publisher
#endif
